home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 60 / IOPROG_60.ISO / soft / c++ / gsl-1.1.1-setup.exe / {app} / src / linalg / hh.c < prev    next >
Encoding:
C/C++ Source or Header  |  2002-04-18  |  4.1 KB  |  180 lines
  1. /* linalg/hh.c
  2.  * 
  3.  * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, Brian Gough
  4.  * 
  5.  * This program is free software; you can redistribute it and/or modify
  6.  * it under the terms of the GNU General Public License as published by
  7.  * the Free Software Foundation; either version 2 of the License, or (at
  8.  * your option) any later version.
  9.  * 
  10.  * This program is distributed in the hope that it will be useful, but
  11.  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.  * General Public License for more details.
  14.  * 
  15.  * You should have received a copy of the GNU General Public License
  16.  * along with this program; if not, write to the Free Software
  17.  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18.  */
  19.  
  20. /* Author:  G. Jungman */
  21.  
  22. #include <config.h>
  23. #include <stdlib.h>
  24. #include <gsl/gsl_math.h>
  25. #include <gsl/gsl_vector.h>
  26. #include <gsl/gsl_matrix.h>
  27. #include <gsl/gsl_linalg.h>
  28.  
  29. #define REAL double
  30.  
  31. /* [Engeln-Mullges + Uhlig, Alg. 4.42]
  32.  */
  33.  
  34. int
  35. gsl_linalg_HH_solve (gsl_matrix * A, const gsl_vector * b, gsl_vector * x)
  36. {
  37.   if (A->size1 > A->size2)
  38.     {
  39.       /* System is underdetermined. */
  40.  
  41.       GSL_ERROR ("System is underdetermined", GSL_EINVAL);
  42.     }
  43.   else if (A->size2 != x->size)
  44.     {
  45.       GSL_ERROR ("matrix and vector sizes must be equal", GSL_EBADLEN);
  46.     }
  47.   else
  48.     {
  49.       int status ;
  50.  
  51.       gsl_vector_memcpy (x, b);
  52.  
  53.       status = gsl_linalg_HH_svx (A, x);
  54.       
  55.       return status ;
  56.     }
  57. }
  58.  
  59. int
  60. gsl_linalg_HH_svx (gsl_matrix * A, gsl_vector * x)
  61. {
  62.   if (A->size1 > A->size2)
  63.     {
  64.       /* System is underdetermined. */
  65.  
  66.       GSL_ERROR ("System is underdetermined", GSL_EINVAL);
  67.     }
  68.   else if (A->size2 != x->size)
  69.     {
  70.       GSL_ERROR ("matrix and vector sizes must be equal", GSL_EBADLEN);
  71.     }
  72.   else
  73.     {
  74.       const size_t N = A->size1;
  75.       const size_t M = A->size2;
  76.       size_t i, j, k;
  77.       REAL *d = (REAL *) malloc (N * sizeof (REAL));
  78.  
  79.       if (d == 0)
  80.     {
  81.       GSL_ERROR ("could not allocate memory for workspace", GSL_ENOMEM);
  82.     }
  83.  
  84.       /* Perform Householder transformation. */
  85.  
  86.       for (i = 0; i < N; i++)
  87.     {
  88.       const REAL aii = gsl_matrix_get (A, i, i);
  89.       REAL alpha;
  90.       REAL f;
  91.       REAL ak;
  92.       REAL max_norm = 0.0;
  93.       REAL r = 0.0;
  94.  
  95.       for (k = i; k < M; k++)
  96.         {
  97.           REAL aki = gsl_matrix_get (A, k, i);
  98.           r += aki * aki;
  99.         }
  100.  
  101.       if (r == 0.0)
  102.         {
  103.           /* Rank of matrix is less than size1. */
  104.               free (d);
  105.               GSL_ERROR ("matrix is rank deficient", GSL_ESING);
  106.         }
  107.  
  108.       alpha = sqrt (r) * GSL_SIGN (aii);
  109.  
  110.       ak = 1.0 / (r + alpha * aii);
  111.       gsl_matrix_set (A, i, i, aii + alpha);
  112.  
  113.       d[i] = -alpha;
  114.  
  115.       for (k = i + 1; k < N; k++)
  116.         {
  117.           REAL norm = 0.0;
  118.           f = 0.0;
  119.           for (j = i; j < M; j++)
  120.         {
  121.           REAL ajk = gsl_matrix_get (A, j, k);
  122.           REAL aji = gsl_matrix_get (A, j, i);
  123.           norm += ajk * ajk;
  124.           f += ajk * aji;
  125.         }
  126.           max_norm = GSL_MAX (max_norm, norm);
  127.  
  128.           f *= ak;
  129.  
  130.           for (j = i; j < M; j++)
  131.         {
  132.           REAL ajk = gsl_matrix_get (A, j, k);
  133.           REAL aji = gsl_matrix_get (A, j, i);
  134.           gsl_matrix_set (A, j, k, ajk - f * aji);
  135.         }
  136.         }
  137.  
  138.       if (fabs (alpha) < 2.0 * GSL_DBL_EPSILON * sqrt (max_norm))
  139.         {
  140.           /* Apparent singularity. */
  141.           free (d);
  142.           GSL_ERROR("apparent singularity detected", GSL_ESING);
  143.         }
  144.  
  145.       /* Perform update of RHS. */
  146.  
  147.       f = 0.0;
  148.       for (j = i; j < M; j++)
  149.         {
  150.           f += gsl_vector_get (x, j) * gsl_matrix_get (A, j, i);
  151.         }
  152.       f *= ak;
  153.       for (j = i; j < M; j++)
  154.         {
  155.           REAL xj = gsl_vector_get (x, j);
  156.               REAL aji = gsl_matrix_get (A, j, i);
  157.           gsl_vector_set (x, j, xj - f * aji);
  158.         }
  159.     }
  160.  
  161.       /* Perform back-substitution. */
  162.  
  163.       for (i = N; i > 0 && i--;)
  164.     {
  165.       REAL xi = gsl_vector_get (x, i);
  166.       REAL sum = 0.0;
  167.       for (k = i + 1; k < N; k++)
  168.         {
  169.           sum += gsl_matrix_get (A, i, k) * gsl_vector_get (x, k);
  170.         }
  171.  
  172.       gsl_vector_set (x, i, (xi - sum) / d[i]);
  173.     }
  174.  
  175.       free (d);
  176.       return GSL_SUCCESS;
  177.     }
  178. }
  179.  
  180.